#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "../../mydevice.h"


void kalman_init(KalmanFilter* kf, float q, float r, float initial_value) {
    kf->q = q;
    kf->r = r;
    kf->x = initial_value;
    kf->p = 1.0f;  // 初始估计误差协方差
    kf->k = 0.0f;
}

float kalman_update(KalmanFilter* kf, float measurement) {
    // 预测步骤 (假设系统模型为x = x)
    kf->p = kf->p + kf->q;

    // 更新步骤
    kf->k = kf->p / (kf->p + kf->r);
    kf->x = kf->x + kf->k * (measurement - kf->x);
    kf->p = (1 - kf->k) * kf->p;

    return kf->x;
}

// 将字符串转换为浮点数
static float str_to_float(const char* str) {
    return atof(str);
}

// 将浮点数转换为字符串，保留两位小数
static void float_to_str(float value, char* buffer, size_t size) {
    snprintf(buffer, size, "%.2f", value);
}

// typedef struct SensorData {
//     char ligBuffer[10];
//     char humidity[10];
//     char temperature[10];
//     char soilHum[10];
//     int id;
// } SensorData;

void process_sensor_data(SensorData* data) {
    // 每个传感器字段使用独立的滤波器（静态变量保持状态）
    static KalmanFilter lig_kf, hum_kf, temp_kf, soil_kf;
    static int initialized = 0;

    if (!initialized) {
        // 初始化滤波器参数（需根据实际噪声调整q和r）
        kalman_init(&lig_kf, 0.1f, 0.5f, str_to_float(data->ligBuffer));
        kalman_init(&hum_kf, 0.1f, 0.5f, str_to_float(data->humidity));
        kalman_init(&temp_kf, 0.1f, 0.5f, str_to_float(data->temperature));
        kalman_init(&soil_kf, 0.1f, 0.5f, str_to_float(data->soilHum));
        initialized = 1;
    }

    // 处理光照数据
    float lig_value = str_to_float(data->ligBuffer);
    float filtered_lig = kalman_update(&lig_kf, lig_value);
    float_to_str(filtered_lig, data->ligBuffer, sizeof(data->ligBuffer));

    // 处理湿度
    float hum_value = str_to_float(data->humidity);
    float filtered_hum = kalman_update(&hum_kf, hum_value);
    float_to_str(filtered_hum, data->humidity, sizeof(data->humidity));

    // 处理温度
    float temp_value = str_to_float(data->temperature);
    float filtered_temp = kalman_update(&temp_kf, temp_value);
    float_to_str(filtered_temp, data->temperature, sizeof(data->temperature));

    // 处理土壤湿度
    float soil_value = str_to_float(data->soilHum);
    float filtered_soil = kalman_update(&soil_kf, soil_value);
    float_to_str(filtered_soil, data->soilHum, sizeof(data->soilHum));
}

/* 示例使用 
int main() {
    SensorData sensor_data = {
        .ligBuffer = "45.0",
        .humidity = "60.5",
        .temperature = "25.3",
        .soilHum = "30.2",
        .id = 1
    };

    // 模拟多次更新以观察滤波效果
    for (int i = 0; i < 5; ++i) {
        process_sensor_data(&sensor_data);
        printf("Filtered Data: Light=%s, Humidity=%s, Temp=%s, Soil=%s\n",
               sensor_data.ligBuffer,
               sensor_data.humidity,
               sensor_data.temperature,
               sensor_data.soilHum);
    }

    return 0;
}*/